#include <WProgram.h>
#include <Messenger.h>
#include <Wire.h>
#include <hmc6352.h>
#include <MegaServo.h>

#include "NBmotors.h"
#include "NBimu.h"
//#include "NBcompass.h"
#include "NBpressure.h"
//#include "NBtemp.h"
#include "NBusb.h"

const int RUNDELAY = 10;
const int HEADINGTOLER = 0;
const float DEPTHTOLER = 0;

const int KILLPIN = 52;


unsigned long timer = 0;

int currentHeading = 0;
int targetHeading = 0;
float currentDepth = 0.0;
float targetDepth = 2.0;
static int previousKillState = LOW;

void setup() 
{
  Wire.begin();
  
  initDigitalKill();

  initUSB();

  initMessenger();

  initMotors();
  
  initPressure();
  
  //initCompass();
  
  initIMUReferenceHeading();
} 

void loop() 
{
  if ((millis() - timer) >= RUNDELAY)
  {
    timer = millis();

    if(autoRun)
    {
      getCmdrMessage();

      getIMUData();
      currentHeading = getIMUHeadingReferenced();
      
      currentDepth = getDepth();

      maintainHeading();
      maintainDepth();

      //Serial.println(currentDepth);
    }
    else
    {
      checkRemoteUSBInput();
    }

    checkDigitalKill();
    
    //sendDataToCmdr();
  }
}

void initDigitalKill()
{
  pinMode(KILLPIN, INPUT);
}

void checkDigitalKill()
{
  int killState = digitalRead(KILLPIN);

  if(killState == LOW)
  {
    autoRun = false;
    
    if( (currentDrive != STOP) || (currentDive != STOP) ||
        (currentFrontLat != STOP) || (currentRearLat != STOP)
      )
    {
      allMotorsKill();
    }
  }
  else if( (killState == HIGH) && (previousKillState == LOW) )
  {
    autoRun = true;
    
    initIMUReferenceHeading();
    
    setDriveMotorsPos(FULLFORWARD2);
  }
  
  previousKillState = killState;
}

void maintainDepth()
{
  //Need to dive: Full Speed
  //Need to rise: Stop
  //Right on target: "Idle" Speed
  if(currentDepth < (targetDepth - DEPTHTOLER) )
    diveMotorsFull();
  else if(currentDepth > (targetDepth + DEPTHTOLER) )
    diveMotorsStop();
  //else
  //  diveMotorsIdle();
}

void maintainHeading()
{
  int delta;
  int correctionAngle;
  int curHeading = currentHeading;
  int tarHeading = targetHeading;
  
  // Normailize the headings
  curHeading = 360 - curHeading;
  tarHeading = 360 - tarHeading;
  
  //Find the difference between the angles
  delta = tarHeading - curHeading;

  // Adjust delta to shortest correction if needed.
  if( delta > 180)
  {
    delta -= 360;
  }
  else if(delta < -180)
  {
    delta += 360;
  }

  //correctionAngle is the shorest angle difference between
  //currentHeading and targetHeading
  correctionAngle = -delta;

  if(correctionAngle < -HEADINGTOLER)
    latTurnLeft();
  else if(correctionAngle > HEADINGTOLER)
    latTurnRight();
  else
    latMotorsKill();
}

void processCmdrMessage()
{
  //int latDC = 0;
  
  char tempChar = cmdr.readChar();

  switch(tempChar)
  {

  case 'H':
    targetHeading = cmdr.readInt();
    //Serial.println(targetHeading, DEC);

    break;

  case 'D':
    targetDepth = ( cmdr.readInt() / 10.0);
    //Serial.println(targetDepth, DEC);

    break;

  case 'S':
    setDriveMotorsPos( convertDCToAngle(cmdr.readInt() ) );
    //Serial.println( convertAngleToDC(leftDrive.read() ) , DEC);

    break;

  case 'F':
    setFrontLatMotorPos( convertDCToAngle( cmdr.readInt() ) );
    //Serial.println( convertAngleToDC(frontLat.read() ) , DEC);

    break;

  case 'R':
    setRearLatMotorPos( convertDCToAngle( cmdr.readInt() ) );
    //Serial.println( convertAngleToDC(frontLat.read() ) , DEC);

    break;

  case 'P':
    autoRun = !autoRun;

    //Serial.print("Autorun: ");
    //Serial.println(autoRun, BIN);

    if(!autoRun)
      allMotorsKill();

    break;
  }
}

void sendDataToCmdr()
{
  Serial.print(autoRun, DEC);
  Serial.print(',');
  Serial.print(currentHeading, DEC);
  Serial.print(',');
  Serial.print(targetHeading, DEC);
  Serial.print(',');
  Serial.print(currentDepth, DEC);
  Serial.print(',');
  Serial.print(targetDepth, DEC);
  Serial.print(',');
  Serial.print(convertAngleToDC(currentDrive), DEC);
  Serial.print(',');
  Serial.print(convertAngleToDC(currentDive), DEC);
  Serial.print(',');
  Serial.print(convertAngleToDC(currentFrontLat), DEC);
  Serial.print(',');
  Serial.print(convertAngleToDC(currentRearLat), DEC);
  Serial.println();
}
